/*----------------------------------------------------------------------------*/
/* Copyright (c) 2013 Worthington Robot Club. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package us.oh.k12.wkw.robot.subsystem;

import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.tables.TableKeyNotDefinedException;
import edu.wpi.first.wpilibj.util.AllocationException;
import us.oh.k12.wkw.robot.command.DoNothingCmd;
import us.oh.k12.wkw.robot.command.DriveWithJoystickCmd;
import us.oh.k12.wkw.robot.command.DriveWithJoysticksCmd;
import us.oh.k12.wkw.robot.command.DriveWithNetworkTableCmd;

/**
 * Represents the robot drive (wheels and related motors) system.
 */
public class DriveSystem extends SystemBase {

    private static final boolean ARCADE_DRIVE_ENABLED = false;
    private static final boolean NETWORK_DRIVE_ENABLED = false;
    // the left drive jaguar and motor.
    private Talon leftDrive;
    // the right drive jaguar and motor.
    private Talon rightDrive;
    // the drive.
    private RobotDrive drive;
    private double leftDriveSpeed = 0;
    private double rightDriveSpeed = 0;
    private Gyro gyro;
    private boolean xTargetPidState = false;
    PIDController xTargetLeftPidController = null;
    PIDController xTargetRightPidController = null;
    PIDController headingPidController = null;
    private int xFilterType = FilterSmoothing.TYPE_ALPHA_BETA;
    private double xFilterAlpha = FilterSmoothing.DEFAULT_ALPHA;
    private double xFilterBeta = FilterSmoothing.DEFAULT_BETA;
    private FilterSmoothing xFilter = null;
    private double xOffset = -50.0;
    private SendableChooser driveCommandChooser;
    private double cameraXRawCurrent = 0;
    private double cameraXRawLast = 0;
    private double cameraXSmoothCurrent = 0;
    private double cameraXSmoothLast = 0;
    private double cameraXReturnCurrent = 0;
    private double cameraXReturnLast = 0;
    private double gyroRawCurrent = 0;
    private double gyroRawLast = 0;
    private double gyroSmoothCurrent = 0;
    private double gyroSmoothLast = 0;
    private int gyroFilterType = FilterSmoothing.TYPE_ALPHA_BETA;
    private double gyroFilterAlpha = FilterSmoothing.DEFAULT_ALPHA;
    private double gyroFilterBeta = FilterSmoothing.DEFAULT_BETA;
    private FilterSmoothing gyroFilter = null;

    //private int logDelay = 0;
    //private static final int LOG_DELAY_START = 20;
    /**
     * The default null constructor.
     */
    public String getName() {
        return "DriveSystem";
    }

    public String getSmartDashboardType() {
        return "DriveSystem";
    }

    public DriveSystem() {

        super("DriveSystem");

        try {

            this.initXfilter();
            this.initGyroFilter();
            this.initDrive();
            this.initXLeftTargetPid();
            this.initXRightTargetPid();

            NetworkTable aTable = NetworkTable.getTable("SmartDashboard");
            aTable.putNumber("WkwFrcLeftSpeed", 0.000000);
            aTable.putNumber("WkwFrcRightSpeed", 0.000000);

            System.out.println("DriveSystem initialized.");

        } catch (AllocationException anAloEx) {
            this.error("DriveSystem()", anAloEx);

        } catch (Exception anEx) {
            this.error("DriveSystem()", anEx);
        }
    }

    private void initXfilter() {

        this.xFilter = new FilterSmoothing(this.xFilterType, this.xFilterAlpha, this.xFilterBeta);

    }

    private void initGyroFilter() {

        this.gyroFilter = new FilterSmoothing(this.gyroFilterType, this.gyroFilterAlpha, this.gyroFilterBeta);

    }

    private void initDrive() {

        // setup the left drive.

        this.leftDrive = new Talon(1);

        LiveWindow.addActuator("DriveSystem", "LeftDrive", this.leftDrive);

        // setup the right drive.

        this.rightDrive = new Talon(2);

        LiveWindow.addActuator("DriveSystem", "RightDrive", this.rightDrive);

        if ((null == this.leftDrive) || (null == this.rightDrive)) {

            this.error("initDrive()", "leftDrive or rightDrive is null.", null);

        } else {

            this.leftDrive.setExpiration(200.0);
            this.leftDrive.setSafetyEnabled(false);

            this.rightDrive.setExpiration(200.0);
            this.rightDrive.setSafetyEnabled(false);

            // setup the robot drive

            this.drive = new RobotDrive(this.leftDrive, this.rightDrive);

            if (null == this.drive) {

                this.error("initDrive()", "drive is null.", null);

            } else {

                // initialize the drive.

                this.drive.setExpiration(200.0);
                this.drive.setSafetyEnabled(false);

            }

        }

        this.gyro = new Gyro(1);
        this.gyro.reset();
        LiveWindow.addSensor("DriveSystem", "Gryo", this.gyro);

    }

    protected void initDefaultCommand() {

        try {

            this.driveCommandChooser = new SendableChooser();
            this.driveCommandChooser.addDefault("DriveWithJoysticksCmd", new DriveWithJoysticksCmd());
            this.driveCommandChooser.addObject("DriveWithJoystickCmd", new DriveWithJoystickCmd());
            this.driveCommandChooser.addObject("DriveWithNetworkTableCmd", new DriveWithNetworkTableCmd());
            this.driveCommandChooser.addObject("DoNothingCmd", new DoNothingCmd());
            SmartDashboard.putData("DriveCommandChooser", this.driveCommandChooser);

            System.out.println("DriveSystem initDefaultCommand called.");

            Command aSelectedCommand = null;

            if (null != this.driveCommandChooser) {

                aSelectedCommand = (Command) this.driveCommandChooser.getSelected();

            }

            if (null == aSelectedCommand) {

                aSelectedCommand = new DriveWithJoysticksCmd();

            }

            this.setDefaultCommand(aSelectedCommand);

        } catch (Exception anEx) {
            this.error("initDefaultCommand()", anEx);
        }

    }

    public void flushStatus() {

        try {

            SmartDashboard.putNumber("XFilterType", this.xFilterType);
            SmartDashboard.putNumber("XFilterAlpha", this.xFilterAlpha);
            SmartDashboard.putNumber("XFilterBeta", this.xFilterBeta);

            SmartDashboard.putNumber("GyroFilterType", this.gyroFilterType);
            SmartDashboard.putNumber("GyroFilterAlpha", this.gyroFilterAlpha);
            SmartDashboard.putNumber("GyroFilterBeta", this.gyroFilterBeta);

            SmartDashboard.putBoolean("XTargetPidEnabled", this.xTargetPidState);

        } catch (Exception anEx) {
            this.error("flushStatus()", anEx);
        }
    }

    public void updateStatus() {

        try {

            if (null != this.leftDrive) {
                if (this.leftDriveSpeed != this.leftDrive.getSpeed()) {
                    this.leftDriveSpeed = this.leftDrive.getSpeed();
                    SmartDashboard.putNumber("DriveLeftMotorSpeed", this.leftDriveSpeed);
                }
            }

            if (null != this.rightDrive) {
                if (this.rightDriveSpeed != this.rightDrive.getSpeed()) {
                    this.rightDriveSpeed = this.rightDrive.getSpeed();
                    SmartDashboard.putNumber("DriveRghtMotorSpeed", this.rightDriveSpeed);
                }
            }

            // force refresh of gyro since it is only used in commands
            this.getGyroHeading();

            if (this.gyroRawLast != this.gyroRawCurrent) {
                this.gyroRawLast = this.gyroRawCurrent;
                SmartDashboard.putNumber("GyroAngleRaw", this.gyroRawCurrent); //this.gyro.getAngle()
            }

            if (this.gyroSmoothLast != this.gyroSmoothCurrent) {
                this.gyroSmoothLast = this.gyroSmoothCurrent;
                SmartDashboard.putNumber("GyroAngleSmooth", this.gyroSmoothCurrent);
            }

            // force refresh of camera data
            this.getCameraXOffset();

            if (this.cameraXRawLast != this.cameraXRawCurrent) {
                this.cameraXRawLast = this.cameraXRawCurrent;
                SmartDashboard.putNumber("CameraXRaw", this.cameraXRawCurrent);
            }

            if (this.cameraXSmoothLast != this.cameraXSmoothCurrent) {
                this.cameraXSmoothLast = this.cameraXSmoothCurrent;
                SmartDashboard.putNumber("CameraXSmooth", this.cameraXSmoothCurrent);
            }

            if (this.cameraXReturnLast != this.cameraXReturnCurrent) {
                this.cameraXReturnLast = this.cameraXReturnCurrent;
                SmartDashboard.putNumber("CameraXReturn", this.cameraXReturnCurrent);
            }

        } catch (Exception anEx) {
            this.error("updateStatus()", anEx);
        }
    }

    private void initXLeftTargetPid() {
        this.xTargetLeftPidController = new PIDController(0.02, 0, 0.0, new XTargetingAnglePidInput(), leftDrive);
        this.xTargetLeftPidController.setSetpoint(0);
        this.xTargetLeftPidController.setInputRange(-250, 250);
        this.xTargetLeftPidController.setOutputRange(-0.8, 0.8);
        this.xTargetLeftPidController.setAbsoluteTolerance(15);
        this.xTargetLeftPidController.disable();
    }

    private void initXRightTargetPid() {
        this.xTargetRightPidController = new PIDController(0.02, 0, 0.0, new XTargetingAnglePidInput(), rightDrive);
        this.xTargetRightPidController.setSetpoint(0);
        this.xTargetRightPidController.setInputRange(-250, 250);
        this.xTargetRightPidController.setOutputRange(-0.8, 0.8);
        this.xTargetRightPidController.setAbsoluteTolerance(15);
        this.xTargetRightPidController.disable();
    }

    private class XTargetingAnglePidInput implements PIDSource {

        public double pidGet() {

            return DriveSystem.this.getCameraXOffset();

        }
    }

    public double getCameraXOffset() {

        try {

            this.cameraXRawCurrent = SmartDashboard.getNumber("x_center");
            this.cameraXSmoothCurrent = this.xFilter.filter(this.cameraXRawCurrent);
            this.cameraXReturnCurrent = (this.cameraXSmoothCurrent + this.xOffset);

        } catch (TableKeyNotDefinedException b) {
            System.out.println("getCameraXOffset() TableKeyNotDefinedException.");
        }

        return this.cameraXReturnCurrent;
    }

    public boolean isXOnTarget() {

        boolean isOnTarget = false;
        double target = this.getCameraXOffset();

        if (target > -5 && target < 5) {
            isOnTarget = true;
        }

        return isOnTarget;
    }

    public void disableXPid() {
        this.xTargetPidState = false;
        this.xTargetLeftPidController.disable();
        this.xTargetRightPidController.disable();
        SmartDashboard.putBoolean("XTargetPidEnabled", this.xTargetPidState);
    }

    public void enableXPid() {
        this.xTargetPidState = true;
        this.xTargetLeftPidController.enable();
        this.xTargetLeftPidController.setContinuous();
        this.xTargetRightPidController.enable();
        this.xTargetRightPidController.setContinuous();
        SmartDashboard.putBoolean("XTargetPidEnabled", this.xTargetPidState);
    }

    public void toggleXTargetPidEnabled() {

        this.xTargetPidState = !this.xTargetPidState;

        if (this.xTargetPidState) {

            this.xTargetLeftPidController.enable();
            this.xTargetLeftPidController.setContinuous();

            this.xTargetRightPidController.enable();
            this.xTargetRightPidController.setContinuous();

        } else {

            this.xTargetLeftPidController.disable();
            this.xTargetRightPidController.disable();

        }

        SmartDashboard.putBoolean("XTargetPidEnabled", this.xTargetPidState);
    }

    public void resetGyro() {
        this.gyro.reset();
        SmartDashboard.putString("gyroReset", "Reset.");
    }

    public double getGyroHeading() {
        this.gyroRawCurrent = this.gyro.getAngle();
        this.gyroSmoothCurrent = this.gyroFilter.filter(this.gyroRawCurrent);
        return this.gyroSmoothCurrent;
    }

    public void driveWithJoystick(final Joystick pJoystick) {

        try {

            // make sure we have the drive object.

            if (null != this.drive) {

                this.drive.arcadeDrive(pJoystick);

            }

        } catch (Exception anEx) {
            this.error("driveWithJoystick()", anEx);
        }
    }

    public void driveWithJoysticks(final Joystick pLeftJoystick, final Joystick pRightJoystick) {

        try {

            // make sure we have the drive object.

            if (null != this.drive) {

                this.drive.tankDrive(pLeftJoystick, pRightJoystick);

            }

        } catch (Exception anEx) {
            this.error("driveWithJoysticks()", anEx);
        }
    }

    public void tankDrive(double pLeft, double pRight) {

        this.drive.tankDrive(pLeft, pRight);

    }

    public void driveWithNetworkTable() {
        double aLeftSpeed = 0;
        double aRightSpeed = 0;

        try {

            NetworkTable aTable = NetworkTable.getTable("SmartDashboard");
            aLeftSpeed = aTable.getNumber("WkwFrcLeftSpeed");
            aRightSpeed = aTable.getNumber("WkwFrcRightSpeed");

            //if (this.logDelay-- < 0) {
            //    this.logDelay = DriveSystem.LOG_DELAY_START;
            //    this.debug("driveWithNetworkTable()", "aLeftSpeed=" + aLeftSpeed + ", aRightSpeed=" + aRightSpeed + ".");
            //}

        } catch (TableKeyNotDefinedException aTndEx) {
            this.error("driveWithNetworkTable()", "TableKeyNotDefinedException.", null);
        }

        this.drive.tankDrive(aLeftSpeed, aRightSpeed);
    }

    public void leftOn(final double pSpeed) {
        if (null != this.leftDrive) {
            this.leftDrive.set(pSpeed);
        }
    }

    public void leftOff() {
        if (null != this.leftDrive) {
            this.leftDrive.set(0);
        }
    }

    public void rightOn(final double pSpeed) {
        if (null != this.leftDrive) {
            this.rightDrive.set(pSpeed);
        }
    }

    public void rightOff() {
        if (null != this.leftDrive) {
            this.rightDrive.set(0);
        }
    }

    public void straight(double pSpeed) {

        try {

            if (null != this.drive) {
                this.drive.tankDrive(pSpeed, pSpeed);
            }

        } catch (Exception anEx) {
            this.error("straight()", anEx);
        }
    }

    public void rotateRight() {

        try {

            if (null != this.drive) {

                this.drive.tankDrive(-0.5, 0.5);
            }

        } catch (Exception anEx) {
            this.error("rotate()", anEx);
        }
    }

    public void rotateLeft() {

        try {

            if (null != this.drive) {

                this.drive.tankDrive(0.5, -0.5);
            }

        } catch (Exception anEx) {
            this.error("rotate()", anEx);
        }
    }

    public void stop() {

        try {

            if (null != this.drive) {

                this.drive.tankDrive(0.000000, 0.000000);
            }

        } catch (Exception anEx) {
            this.error("stop()", anEx);
        }
    }

    public void turnLeft90() {

        try {

            if (null != this.drive) {

                this.drive.tankDrive(0.0, 1.0);
            }

        } catch (Exception anEx) {
            this.error("turnLeft90()", anEx);
        }
    }

    public void turnRight90() {

        try {

            if (null != this.drive) {

                this.drive.tankDrive(1.0, 0.0);
            }

        } catch (Exception anEx) {
            this.error("turnRight90()", anEx);
        }
    }

    public double getLeftDriveSpeed() {
        return (null == this.leftDrive ? 0 : this.leftDrive.getSpeed());
    }

    public double getRightDriveSpeed() {
        return (null == this.rightDrive ? 0 : this.rightDrive.getSpeed());
    }
}
